const SQUARE_HREF = (Site.DEBUG) ? 'http://localhost:8081/live3dstatic/square.dae' : 'http://www.projectlive3d.com/live3dstatic/square.dae'


function PTZCalibration(baseURL, origin, cameraPosition,
                        focalLengthF1, focalLengthF2, focalLengthF3,
						panOffsetRad, tiltOffsetRad, rollOffsetRad,
						panRanges, tiltRanges, zoomRanges,
						totalPanRad, totalTiltRad,
						width, height,
						imageURL){
	this.baseURL = baseURL;
	this.origin = origin;
	this.focalLengthF1 = focalLengthF1;
	this.focalLengthF2 = focalLengthF2;
	this.focalLengthF3  = focalLengthF3;
	this.panOffsetRad = panOffsetRad;
	this.tiltOffsetRad = tiltOffsetRad;
	this.rollOffsetRad = rollOffsetRad;
	this.cameraPosition = cameraPosition;
	this.width = width;
	this.height = height;
	this.panRanges = panRanges;
	this.tiltRanges = tiltRanges;
	this.zoomRanges = zoomRanges;
	this.totalPanRad = totalPanRad,
	this.totalTiltRad = totalTiltRad,
	this.currentPan = null;
	this.currentTilt = null;
	this.currentZoom = null;
	this.origin = origin;
	this.imageURL = imageURL;
	
	this.refreshModel = this.makeSquareModel(origin.lat, origin.lon, origin.alt, imageURL, true);
	this.imageModel = this.makeSquareModel(origin.lat, origin.lon, origin.alt, imageURL, false);
	ge.getFeatures().appendChild(this.refreshModel)
	ge.getFeatures().appendChild(this.imageModel)
	
	
	var me = this;
	this.ready = false;
	this.updatePTZValues(function(){
						 me.ready = true}, null);
	
	this.refresh();
}

PTZCalibration.prototype.refresh = function(){
	var me = this;
	if (! this.ready)
		return
		
	this.updatePTZValues(function(){
							me.updateM();
						 me.drawFrustum();
						});
}

PTZCalibration.prototype.updatePTZValues = function(readyFn, params){
	var http = createRequest();
	var url = "getCurrPtz?baseURL=" + this.baseURL;
	http.open("GET", url, true);
	var me = this;
	http.onreadystatechange = function(){
		if(http.readyState == 4){
			var ptzString = http.responseText;
			var ptz = ptzString.split(" ");
			me.currentPan = Number(ptz[0]);
			me.currentTilt = Number(ptz[1]);
			me.currentZoom = Number(ptz[2]);
			readyFn(params);
		}
	};
	http.send(null);
	
}

PTZCalibration.prototype.updateM = function(){
	var panNormal  = (this.currentPan  - this.panRanges[0])  / (this.panRanges[1] - this.panRanges[0]);
	var tiltNormal = (this.currentTilt - this.tiltRanges[0]) / (this.tiltRanges[1] - this.tiltRanges[0]);
		
	// assuming linear camera response.
	/*
	var c = this.currentZoom;
	// K is the camera intrinsic matrix.
	//var focal = this.focalLengthF1 + this.focalLengthF2 * c + 0.5 * (this.focalLengthF3 * c * c);
	var focal = this.focalLengthF1 / (c - this.focalLengthF2) + this.focalLengthF3;
	*/
	
	var a = this.focalLengthF1;
    var b = this.focalLengthF2;
    var c = this.focalLengthF3;
    var zmax = this.zoomRanges[0]
    var zmin = this.zoomRanges[1]
    var zh = (zmax + zmin) / 2;

    var sub1 = -a * b * zh + b * c * zh + a * c * zmax - b * c * zmax + a * b * zmin -a * c * zmin;
    var sub2 = -a * zh + c * zh + a * zmax - b * zmax + b * zmin -c * zmin;
    var sub3 = b * zh * zmax - c * zh * zmax + a * zh * zmin - b * zh * zmin - a * zmax * zmin +c * zmax * zmin;

    var beta = (sub3)/(-sub2)
    var gamma = (sub1)/(sub2)
    var alpha = b * zh - zh * gamma - b * beta + beta * gamma;
    
    var focal = alpha / (this.currentZoom - beta) + gamma;
	
	var K = new Matrix(3,3, [[focal,0, this.width / 2], 
							 [0,-focal, this.height / 2],
							 [0,0,1]]);
	this.panRadians = -(panNormal * this.totalPanRad + this.panOffsetRad);
	this.tiltRadians = tiltNormal * this.totalTiltRad + this.tiltOffsetRad;
	
	if (this.panRadians > 2 * Math.PI){
		this.panRadians = this.panRadians - 2 * Math.PI;
	} else if (this.panRadians < 0){
		this.panRadians = this.panRadians + 2 * Math.PI;
	}
	
	if (this.tiltRadians > 2 * Math.PI){
		this.tiltRadians = this.tiltRadians - 2 * Math.PI;
	} else if (this.tiltRadians < 0){
		this.tiltRadians = this.tiltRadians + 2 * Math.PI;
	}
		
	// R is the camera rotation matrix.
	var ca = Math.cos(this.panRadians);
    var cb = Math.cos(this.tiltRadians);
	var cg = Math.cos(this.rollOffsetRad + PI);
    
    var sa = Math.sin(this.panRadians);
    var sb = Math.sin(this.tiltRadians);
	var sg = Math.sin(this.rollOffsetRad + PI);
	
    var R = new Matrix(3,3,[[ca * cg - cb * sa * sg, cg * sa + ca * cb * sg, sb * sg],
						    [-cb * cg * sa - ca * sg, ca * cb * cg - sa*sg, cg * sb],
							[sa * sb, -ca * sb, cb]]);
	
	// T is the camera translation matrix.
	var T = new Matrix(1,3, [[this.cameraPosition.x], [this.cameraPosition.y], [this.cameraPosition.z]]);
	T = R.multiply(T);
	R = R.values;
	T = T.values;
	T[0][0] = -T[0][0];
	T[1][0] = -T[1][0];
	T[2][0] = -T[2][0];
	
	var M = new Matrix(4,3,[[R[0][0], R[0][1], R[0][2], T[0][0]],
					        [R[1][0], R[1][1], R[1][2], T[1][0]],
						    [R[2][0], R[2][1], R[2][2], T[2][0]]]);
	M = K.multiply(M);
	
	this.M = M
}

// if montage is set to true, then no previous images will be erased.
PTZCalibration.prototype.drawFrustum = function(){
	
	if (this.singleCalib){
		this.singleCalib.undrawFrustum();
	}
	
	
	var headingDeg = - this.panRadians * 180 / PI;
	var tiltDeg = this.tiltRadians * 180 / PI;
	headingDeg = headingDeg;
	tiltDeg = tiltDeg;

	//var rollDeg = this.rollOffsetRad * 180 / PI;
	var rollDeg = 0;
	this.singleCalib = new Calibration(this.M, this.cameraPosition, tiltDeg, headingDeg, this.origin, this.width, this.height);
	this.singleCalib.drawFrustum()
	
	var updateView = document.getElementById('autoReset').checked;
	if (updateView){
		jumpToFull(this.cameraPosition.lat, this.cameraPosition.lon, this.cameraPosition.alt, rollDeg, tiltDeg, headingDeg);
	}
	
	if (document.getElementById('montageBox')){
		var montage = document.getElementById('montageBox').checked;
		var takePicture = document.getElementById('montageReadyBox').checked;
	}
	
	if (!montage){
		this.singleCalib.fitModelToFrustum(this.imageModel, headingDeg, 180-tiltDeg, 0);
		this.singleCalib.fitModelToFrustum(this.refreshModel, headingDeg, 180-tiltDeg, 0);
	} else if (takePicture) {
		document.getElementById('montageReadyBox').checked = false;
		var model = this.makeSquareModel(this.origin.lat, this.origin.lon, this.origin.alt, this.imageURL + '?' + Math.random(), false);
		this.singleCalib.fitModelToFrustum(model, headingDeg, 180-tiltDeg, 0);
		ge.getFeatures().appendChild(model)
	}
}

PTZCalibration.prototype.makeSquareModel = function(lat,lon,alt, url, refresh){
	var kmlString =   
		'<Placemark>'+
		'  <Model>'+
		'    <altitudeMode>absolute</altitudeMode>'+
		'    <Location>'+
		'      <longitude>' + lon + '</longitude>'+
		'      <latitude>' + lat + '</latitude>'+
		'      <altitude>' + (alt + 1) + '</altitude>'+
		'    </Location>'+
		'    <Link>'+
		'      <href>' + SQUARE_HREF + '</href>'+
		'    </Link>' +
		'    <ResourceMap>'+
		'      <Alias>'+
		'        <targetHref>' + url +'</targetHref>'+
		'        <sourceHref>image.jpg</sourceHref>'+
		'      </Alias>'+
		'    </ResourceMap>'+
		'  </Model>'+
		'</Placemark>';
	
	var placemark = ge.parseKml(kmlString);
	
	if (refresh){
		var link = placemark.getGeometry().getLink();
		link.setRefreshMode(ge.REFRESH_ON_INTERVAL);
		link.setRefreshInterval(1.0);
	}
	return placemark;
}